/** Fichier implémentatant des fontions d'aide à la création de matrices de
 * rotation.
 *
 * Ce fichier contient l'implémentatio des fonctions de construction de matrice.
 *
 * @file ConstruitMatrice.cpp
 *
 * @author Johan "Solo" Luisier
 * @date 2010
 */
#include "ConstruitMatrice.hpp"

#include <cmath>

#include "Utils/Erreur.hpp"

#include "Utils/debugstreams.hpp"

using namespace std;

StatusCode axeZ( const VecteurU& zPrime, MatriceSO3& mat )
{
    VecteurU xPrime;
    VecteurU yPrime;

    bool encore( false );

    /**
     * 1er essai : on constuit xPrime normal à zPrime avec une composante Oz
     * nulle.
     */
    xPrime = VecteurU( zPrime.y(), -zPrime.x(), 0 );
    yPrime = zPrime ^ xPrime;

    try
    {
        mat = MatriceSO3( xPrime, yPrime, zPrime );
    }
    catch ( Erreur& e )
    {
        warning() << e.message << endl;
        encore = true;
    }

    if ( ! encore )
        return StatusCode();

    encore = false;

    /**
     * 2ème essai : on constuit xPrime normal à zPrime avec une composante Oy
     * nulle.
     */
    xPrime = VecteurU( zPrime.z(), 0, -zPrime.x() );
    yPrime = zPrime ^ xPrime;

    try
    {
        mat = MatriceSO3( xPrime, yPrime, zPrime );
    }
    catch ( Erreur& e )
    {
        warning() << e.message << endl;
        encore = true;
    }

    if ( ! encore )
        return StatusCode();

    encore = false;

    /**
     * 3ème essai : on constuit xPrime normal à zPrime avec une composante Ox
     * nulle.
     */
    xPrime = VecteurU( 0, zPrime.z(), -zPrime.y() );
    yPrime = zPrime ^ xPrime;

    try
    {
        mat = MatriceSO3( xPrime, yPrime, zPrime );
    }
    catch ( Erreur& e )
    {
        error() << e.message << endl;
        encore = true;
    }

    if ( ! encore )
        return StatusCode();

    /**
     * Si les trois essais ont tous échoués, alors il n'y a rien à faire.
     */
    return StatusCode( false, 0, 0,
                       "Impossible de créer une matrice de rotation" );
}

StatusCode axesXY( const VecteurU& xPrime, const VecteurU& yPrime,
                   MatriceSO3& mat )
{
    if ( abs( xPrime * yPrime ) > 1.e-7 )
        return StatusCode( false, 0, 0, "Les vecteurs ne sont pas orthogonaux" );

    /**
     * On construit zPrime comme le produit vectoriel des deux axes donnés.
     */
    VecteurU zPrime( xPrime ^ yPrime );

    try
    {
        mat = MatriceSO3( xPrime, yPrime, zPrime );
    }
    catch ( Erreur& e )
    {
        error() << e.message << endl;
        return StatusCode( false, 0, 0,
                           "Impossible de créer une matrice de rotation" );
    }

    return StatusCode();
}


StatusCode axesYZ( const VecteurU& yPrime, const VecteurU& zPrime,
                   MatriceSO3& mat )
{
    if ( abs( yPrime * zPrime ) > 1.e-7 )
        return StatusCode( false, 0, 0, "Les vecteurs ne sont pas orthogonaux" );

    /**
     * On construit xPrime comme le produit vectoriel des deux axes donnés.
     */
    VecteurU xPrime( yPrime ^ zPrime );

    try
    {
        mat = MatriceSO3( xPrime, yPrime, zPrime );
    }
    catch ( Erreur& e )
    {
        error() << e.message << endl;
        return StatusCode( false, 0, 0,
                           "Impossible de créer une matrice de rotation" );
    }

    return StatusCode();
}
